// Devin Koepl, Kevin Kemper

#include <fca_controllers/fca_interface_headless.h>

//******************************************************************************
// argv index locations
#define CONTROLLER_IDX	1
#define LOG_IDX			2
#define TIME_START_IDX	3
#define TIME_STOP_IDX	4
#define PARMS_IDX		5

//******************************************************************************

// SHM interface to kernel.

pthread_t datalogging_thread;
ControllerData global_data;

static Shm * shm;

static FILE *log_p = NULL;

//static int logging_flag = 0;
static float time_start	= 0;
static float time_end	= 0;


//******************************************************************************

int main (int argc, char **argv)
{
	int i;


	//**************************************************************************

	// Connect to kernel's shm.

	if ( !( shm = ( Shm * )rt_shm_alloc( nam2num( SHM_NAME ), 0, USE_VMALLOC ) ) )
		ROS_ERROR( "rtai_malloc() data to user space failed (maybe /dev/rtai_shm is missing)!" );

	ROS_INFO( "Connected to SHM." );

	//**************************************************************************
	// init vars
	global_data.command					= CMD_DISABLE;
	global_data.controller_requested	= 0;

	
	//**************************************************************************
	//	Interpret args
	if (argc < PARMS_IDX) { ROS_ERROR("Bad arguments"); return -1;}
	

	global_data.command = CMD_RUN;
	global_data.controller_requested = atoi(argv[CONTROLLER_IDX]);
//	printf("global_data.controller_requested %d\n",global_data.controller_requested);
	log_p = fopen( argv[LOG_IDX], "w" );
	if (log_p == NULL) {
		ROS_ERROR("Bad log file name");
		rt_shm_free( nam2num( SHM_NAME ) );
		ROS_INFO("\nDisconnected from SHM.");
		return -1;
	}


	time_start	= atof(argv[TIME_START_IDX]);
	time_end	= atof(argv[TIME_STOP_IDX]);
	

	switch (global_data.controller_requested)
	{
		case 0:	// reset
			ROS_INFO("CMD_RESTART");
			global_data.command = CMD_RESTART;
			break;
			
		case MOTOR_TORQUE_CONTROLLER:	// 1
			if (argc < PARMS_IDX+1) { ROS_ERROR("Bad # of arguments\n"); return -1;}
			ROS_INFO("MOTOR_TORQUE_CONTROLLER");
			((MtrTrqControllerData *)(&(global_data.data)))->mtr_trq			= atof(argv[PARMS_IDX+0]);
			break;
			
		case MOTOR_POSITION_CONTROLLER:	// 2
			if (argc < PARMS_IDX+3) { ROS_ERROR("Bad # of arguments\n"); return -1;}
			ROS_INFO("MOTOR_POSITION_CONTROLLER");
			((MtrPosControllerData *)(&(global_data.data)))->mtr_ang			= atof(argv[PARMS_IDX+0]);
			((MtrPosControllerData *)(&(global_data.data)))->p_gain				= atof(argv[PARMS_IDX+1]);
			((MtrPosControllerData *)(&(global_data.data)))->d_gain				= atof(argv[PARMS_IDX+2]);
			break;
			
		case FORCE_CONTROLLER: // 3
			if (argc < PARMS_IDX+8) { ROS_ERROR("Bad # of arguments\n"); return -1;}
			ROS_INFO("FORCE_CONTROLLER");s
			((ForceControllerData *)(&(global_data.data)))->torque_cmd			= atof(argv[PARMS_IDX+0]);
			((ForceControllerData *)(&(global_data.data)))->pos_cmd				= atof(argv[PARMS_IDX+1]);
			((ForceControllerData *)(&(global_data.data)))->tp_ratio			= atof(argv[PARMS_IDX+2]);
			((ForceControllerData *)(&(global_data.data)))->trq_lim				= atof(argv[PARMS_IDX+3]);
			((ForceControllerData *)(&(global_data.data)))->torque_p_gain		= atof(argv[PARMS_IDX+4]);
			((ForceControllerData *)(&(global_data.data)))->torque_d_gain		= atof(argv[PARMS_IDX+5]);
			((ForceControllerData *)(&(global_data.data)))->torque_f_gain		= atof(argv[PARMS_IDX+6]);
			((ForceControllerData *)(&(global_data.data)))->pos_p_gain			= atof(argv[PARMS_IDX+7]);
			((ForceControllerData *)(&(global_data.data)))->pos_d_gain			= atof(argv[PARMS_IDX+8]);
			break;
			
		case TORQUE_SINE_CONTROLLER: // 4
			if (argc < PARMS_IDX+8) { ROS_ERROR("Bad # of arguments\n"); return -1;}
			ROS_INFO("FORCE_SINE_CONTROLLER");
			((TorqueSineControllerData *)(&(global_data.data)))->trq_amp		= atof(argv[PARMS_IDX+0]);
			((TorqueSineControllerData *)(&(global_data.data)))->trq_freq		= atof(argv[PARMS_IDX+1]);
			((TorqueSineControllerData *)(&(global_data.data)))->trq_off		= atof(argv[PARMS_IDX+2]);
			((TorqueSineControllerData *)(&(global_data.data)))->trq_k1			= atof(argv[PARMS_IDX+3]);
			((TorqueSineControllerData *)(&(global_data.data)))->trq_k2			= atof(argv[PARMS_IDX+4]);
			((TorqueSineControllerData *)(&(global_data.data)))->trq_ke1		= atof(argv[PARMS_IDX+5]);
			((TorqueSineControllerData *)(&(global_data.data)))->trq_ke2		= atof(argv[PARMS_IDX+6]);
			((TorqueSineControllerData *)(&(global_data.data)))->trq_lim		= atof(argv[PARMS_IDX+7]);
			break;
			
		case POSITION_SINE_CONTROLLER: // 5
			if (argc < PARMS_IDX+7) { ROS_ERROR("Bad # of arguments\n"); return -1;}
			ROS_INFO("POSITION_SINE_CONTROLLER");
			((PositionSineControllerData *)(&(global_data.data)))->pos_amp		= atof(argv[PARMS_IDX+0]);
			((PositionSineControllerData *)(&(global_data.data)))->pos_freq		= atof(argv[PARMS_IDX+1]);
			((PositionSineControllerData *)(&(global_data.data)))->pos_off		= atof(argv[PARMS_IDX+2]);
			((PositionSineControllerData *)(&(global_data.data)))->pos_k1		= atof(argv[PARMS_IDX+3]);
			((PositionSineControllerData *)(&(global_data.data)))->pos_k2		= atof(argv[PARMS_IDX+4]);
			((PositionSineControllerData *)(&(global_data.data)))->pos_ke1		= atof(argv[PARMS_IDX+5]);
			((PositionSineControllerData *)(&(global_data.data)))->pos_ke2		= atof(argv[PARMS_IDX+6]);
			break;
		
		case OPENLOOP_SINE_CONTROLLER: // 5
			if (argc < PARMS_IDX+6) { ROS_ERROR("Bad # of arguments\n"); return -1;}
			ROS_INFO("OPENLOOP_SINE_CONTROLLER");
			((OpenloopSineControllerData *)(&(global_data.data)))->open_k		= atof(argv[PARMS_IDX+0]);
			((OpenloopSineControllerData *)(&(global_data.data)))->open_B		= atof(argv[PARMS_IDX+1]);
			((OpenloopSineControllerData *)(&(global_data.data)))->open_I		= atof(argv[PARMS_IDX+2]);
			((OpenloopSineControllerData *)(&(global_data.data)))->open_f		= atof(argv[PARMS_IDX+3]);
			((OpenloopSineControllerData *)(&(global_data.data)))->open_a		= atof(argv[PARMS_IDX+4]);
			((OpenloopSineControllerData *)(&(global_data.data)))->open_lim		= atof(argv[PARMS_IDX+5]);
			break;
			
		default :
			ROS_ERROR("BAD CONTROLLER INDEX");
			fclose(log_p);
			rt_shm_free( nam2num( SHM_NAME ) );
			ROS_INFO("\nDisconnected from SHM.");
			return -1;
			break;
			
	}

	//**************************************************************************
	// Tell kernel to switch controllers
	printf( "\tDisable\n" );
	shm->controller_data[1 - shm->control_index].command				= CMD_DISABLE;
	shm->controller_data[1 - shm->control_index].controller_requested	= 0;

	for (i = 0; i < SIZE_OF_CONTROLLER_DATA; i++)
		shm->controller_data[1 - shm->control_index].data[i] = global_data.data[i];
	
	// signal the kernel
	shm->req_switch		= true;
	while (shm->req_switch == true) {											// wait until the kernel acks
		usleep(100);
	}

//	usleep(500);


	printf( "\tReset\n" );
	//**************************************************************************
	// Tell kernel to switch controllers
//	while ( shm->w_status == 1) {
		shm->controller_data[1 - shm->control_index].command				= CMD_RESTART;
		shm->controller_data[1 - shm->control_index].controller_requested	= 0;

		for (i = 0; i < SIZE_OF_CONTROLLER_DATA; i++)
			shm->controller_data[1 - shm->control_index].data[i] = global_data.data[i];
	
		// signal the kernel
		shm->req_switch		= true;
		while (shm->req_switch == true) {											// wait until the kernel acks
			usleep(100);
		}
//	}
	
	
	shm->controller_data[1 - shm->control_index].command				= CMD_DISABLE;
	shm->controller_data[1 - shm->control_index].controller_requested	= 0;

	for (i = 0; i < SIZE_OF_CONTROLLER_DATA; i++)
		shm->controller_data[1 - shm->control_index].data[i] = global_data.data[i];
	
	// signal the kernel
	shm->req_switch		= true;
	while (shm->req_switch == true) {											// wait until the kernel acks
		usleep(100);	
	}
	
	
	printf( "\tWaiting for reset to finish." );
	while (shm->w_status == 0 ) {												// wait until the wrapper is ready
		printf( "." );
		usleep(10000);
	}
	printf( "\n" );
	
	printf( "\tSwitch controllers\n" );
	//**************************************************************************
	// Tell kernel to switch controllers
	shm->controller_data[1 - shm->control_index].command				= CMD_DISABLE;
	shm->controller_data[1 - shm->control_index].controller_requested	= global_data.controller_requested;

	for (i = 0; i < SIZE_OF_CONTROLLER_DATA; i++)
		shm->controller_data[1 - shm->control_index].data[i] = global_data.data[i];
	
	// signal the kernel
	shm->req_switch		= true;
	while (shm->req_switch == true) {											// wait until the kernel acks
		usleep(100);	
	}

	usleep(100000);


	//**************************************************************************
	if ( global_data.command != CMD_RESTART ) {									// start the logger
		pthread_create( &datalogging_thread, NULL, datalogging_task, NULL );
		ROS_INFO( "Waiting for threads to finish." );
	}

	//**************************************************************************
	// Load request into the kernel buffer to start the controller
	shm->controller_data[1 - shm->control_index].command				= CMD_RUN;
	shm->controller_data[1 - shm->control_index].controller_requested	= global_data.controller_requested;

	for (i = 0; i < SIZE_OF_CONTROLLER_DATA; i++)
		shm->controller_data[1 - shm->control_index].data[i] = global_data.data[i];
	
	// signal the kernel
	shm->req_switch		= true;
	while (shm->req_switch == true) {											// wait until the kernel acks
		usleep(100);	
	}

	//**************************************************************************
	
	if (datalogging_thread != NULL)
		pthread_join( datalogging_thread, NULL );								// wait for the logger to finish
		
	
	fclose(log_p);																// close the file
	ROS_INFO( "Finished." );

	//**************************************************************************
	//	Send a final command
	shm->controller_data[1 - shm->control_index].command				= CMD_DISABLE;
	shm->controller_data[1 - shm->control_index].controller_requested	= 0;

	// signal the kernel
	shm->req_switch		= true;
	while (shm->req_switch == true) {											// wait until the kernel acks
		usleep(100);
	}
	
	//**************************************************************************

	// Disconnect from kernel's shm.

	rt_shm_free( nam2num( SHM_NAME ) );

	ROS_INFO("\nDisconnected from SHM.");

	//**************************************************************************

	return 0;
}



//******************************************************************************

// Log a data entry.

void log_data_entry( FILE * fp, int i )
{
	ControllerInput		*c_in;
	ControllerOutput	*c_out;

	c_in	= &shm->controller_input[i];
	c_out 	= &shm->controller_output[i];

	fprintf( fp, "%f, %u, %u, %f, %f, %f, %f, %f, %u\n",
				c_out->time,
				c_in->motor_cnt,
				c_in->leg_cnt,
				c_in->motor_angle,
				c_in->leg_angle,
				c_in->motor_velocity,
				c_in->leg_velocity,
				c_out->motor_torque,
				c_in->command );
}

//******************************************************************************
// Datalog while the robot is running.

void * datalogging_task( void * arg )
{
	ControllerInput		*c_in;
	ControllerOutput	*c_out;

	int i = 0;
	float tmp_time;


	if (log_p == NULL) {
		pthread_exit( NULL );
	}
	
	
	// This print cannot be a ROS_INFO, because of a problem in the ROS header file?
	//ROS_INFO( "Writing %u log entries to %s.", shm->io_index, QUOTEME(LOG_FILENAME) );

	// Create file header.
	fprintf( log_p, "Seconds, Motor Count, Leg Count, Motor Angle, Leg Angle, Motor Velocity, Leg Velocity, Motor Torque, Command\n");

	c_in	= &shm->controller_input[i];
	c_out 	= &shm->controller_output[i];
	tmp_time = c_out->time;

//	printf("shm->io_index %u: c_out->time %f\n",shm->io_index,c_out->time);
	
	while (shm->io_index > time_end*1000) {										// wait for the index to reset
		pthread_yield();
		ros::spinOnce;
	}

	c_in	= &shm->controller_input[i];
	c_out 	= &shm->controller_output[i];
	
	while ( c_out->time < time_end ) {
		
		// try to catch up to the kernel controller
		if (i!= shm->io_index) {
		
			c_in	= &shm->controller_input[i];
			c_out 	= &shm->controller_output[i];
//			printf("i %d: shm->io_index %u: c_out->time %f\n",i,shm->io_index,c_out->time);
			
			if (c_out->time > time_start) {
				fprintf( log_p, "%f, %u, %u, %f, %f, %f, %f, %f, %u\n",
								c_out->time,
								c_in->motor_cnt,
								c_in->leg_cnt,
								c_in->motor_angle,
								c_in->leg_angle,
								c_in->motor_velocity,
								c_in->leg_velocity,
								c_out->motor_torque,
								c_in->command );
			}
			
			i = (++i) % SHM_TO_USPACE_ENTRIES;
		}

		pthread_yield();
		ros::spinOnce;

	}

	
	pthread_exit( NULL );
}


////////////////////////////////////////////////////////////////////////////////
//		GUI functions


void restart_robot( void )
{
	if ( global_data.command == CMD_DISABLE )
	{
		global_data.command					= CMD_RESTART;
		global_data.controller_requested	= NO_CONTROLLER;
	}
}

void enable_motors( void )
{
	global_data.command = CMD_RUN;
}

void disable_motors( void )
{
	global_data.command = CMD_DISABLE;
}



